#ifndef LIDAR_UNDISTORT_H
#define LIDAR_UNDISTORT_H

//根据imu数据对激光数据进行运动畸变校正
#include"def.h"
#include<vector>
using namespace std;

class CImuIntegration
{

public:
    CImuIntegration();
    ~CImuIntegration();
    
    int  AddImu(ImuData& imu);
     //根据起始索引和时间戳来插值得到3个旋转角度
    int  FindAngle(int start, double time, double& ax, double& ay, double& az);
    //根据起始索引和时间戳来插值得到旋转四元数
    int  InterpolateQuaternion(int start, double time, Eigen::Quaterniond& iq);
    
private:
    vector<ImuData> mvImuData;
};

class CLidarUnDistort
{
public:
    CLidarUnDistort();
    ~CLidarUnDistort();
    
    void Clear() { mvImuData.clear(); }
    void SetCaliMatrix(Eigen::Matrix3d rot);

    int  AddImu(ImuData& imu);
    //int  AddScan(pcl::PointCloud<PointXYZIRT>& scan);

    //int  ImuIntegration();

    //根据起始索引和时间戳来插值得到3个旋转角度
    int  FindAngle(int start, double time, double& ax, double& ay, double& az);

    //根据起始索引和时间戳来插值得到旋转四元数
    int  InterpolateQuaternion(int start, double time, Eigen::Quaterniond& iq);

    int  Process(pcl::PointCloud<PointXYZIRT>& scan, double initStamp);

    //根据前后时间戳，基于IMU来计算相对旋转
    Eigen::Affine3f PredictIncreRot(double beginStamp, double endStamp);

private:
    vector<ImuData> mvImuData;
    //vector< pcl::PointCloud<PointXYZIRT> > mvLidarScans;
    Eigen::Matrix3d mRotFromImu2Lidar;  //rotation from imu to lidar
};


#endif

